#include <iostream>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main()
{
    // define rotation matrix by angle-axis
    AngleAxisf r;
    r = AngleAxisf(0.25 * M_PI, Vector3f::UnitZ()) 
        * AngleAxisf(0.5 * M_PI, Vector3f::UnitY()) 
        * AngleAxisf(0.33 * M_PI, Vector3f::UnitX());

    cout << "axis: " << r.axis().transpose() << endl;
    cout << "angle: " << r.angle() << endl;
    cout << "rotation matrix: \n" << r.matrix() << endl; 

    // convert angle-axis to a matrix
    Matrix3f m;
    m = r;
    cout << m << endl
         << "is unitary: " << m.isUnitary() << endl;

    // rotatation by angle-axis is the same to the one by matrix
    Vector3f v(1, 0, 0);
    Vector3f rotated_v = r * v;
    cout << "rotated by r: " << rotated_v.transpose() << endl;

    rotated_v = m * v;
    cout << "rotated by m: " << rotated_v.transpose() << endl;

    // Vector3f euler_angles = m.eulerAngles(2, 1, 0); // ZYX: yaw-pitch-roll
    Vector3f euler_angles = m.eulerAngles(0, 1, 2);
    cout << "euler angles: " << euler_angles.transpose() << endl;

    return 0;
}